#include "Constant.h"
#include "Entity.h"
#include "EntityGenerator.h"
#include "ForceGenerator.hpp"
#include "ForceGeneratorDrag.h"
#include "ForceGeneratorFriction.h"
#include "ForceGeneratorGravity.h"
#include "ForceGeneratorSpring.h"
#include "SimulationManager.h"
#include "Vector3.hpp"

using namespace std;

SimulationManager::SimulationManager()
{
	entityList = new map<int, Entity*>();
	constraints = new map<ConstraintType, bool>();
	constraints->insert(pair<ConstraintType, bool>(GEOMETRIC, false));
	boundVolMin = 0;
	boundVolMax = 0;

	constraints->insert(pair<ConstraintType, bool>(TIME, false));
	lifeTimeLimit = 0.0f;

	generators = new map<ForceGeneratorType, ForceGenerator*>();
	//generators->insert(pair<ForceGeneratorType, ForceGenerator*>(DRAG, new ForceGeneratorDrag(0.1f, 0.1f)));
	//generators->insert(pair<ForceGeneratorType, ForceGenerator*>(FRICTION, new ForceGeneratorFriction()));
	generators->insert(pair<ForceGeneratorType, ForceGenerator*>(GRAVITY, new ForceGeneratorGravity(new Vector3(0.0f, -2.0f, 0.0f))));
	//generators->insert(pair<ForceGeneratorType, ForceGenerator*>(SPRING, new ForceGeneratorSpring(1.0f, 1.0f)));
}

SimulationManager::~SimulationManager()
{	}

Entity* SimulationManager::AddEntity(int id, float mass, Vector3 *position, Vector3 *initForce)
{
	if (entityList->find(id) == entityList->end())
	{
		Entity* tmp = new Entity(mass, position, initForce);
		entityList->insert(pair<int, Entity*>(id, tmp));
		return tmp;	
	}
	return 0;
}


int SimulationManager::Count()
{
	return entityList->size();
}

//Define constraints
void SimulationManager::DefineConstraint(float maxLifeTime)
{
	lifeTimeLimit = maxLifeTime;
	(*constraints)[TIME] = true;
}

void SimulationManager::DefineConstraint(Vector3* boundingMin, Vector3* boundingMax)
{
	boundVolMin = boundingMin;
	boundVolMax = boundingMax;
	(*constraints)[GEOMETRIC] = true;
}


void SimulationManager::DisableConstraint(ConstraintType type)
{
	switch(type)
	{
	case GEOMETRIC:
		boundVolMin = 0;
		boundVolMax = 0;
		break;
	case TIME:
		lifeTimeLimit = 0.0f;
		break;
	}

	(*constraints)[type] = false;
}

Entity* SimulationManager::GetEntity(int id)
{
	if (entityList->find(id) != entityList->end())
	{
		return (*entityList)[id];
	}
	return 0;
}

map<int, Entity*>* SimulationManager::GetEntityList()
{
	return entityList;
}

ForceGenerator* SimulationManager::GetGenerator(ForceGeneratorType type)
{
	return (*generators)[type];
}

void SimulationManager::RegisterEntity(ForceGeneratorType type, int id)
{
	if (entityList->find(id) != entityList->end())
	{
		(*generators)[type]->Register(id, (*entityList)[id]);
	}
}

void SimulationManager::RegisterAllEntities(ForceGeneratorType type)
{
	for(map<int, Entity*>::iterator it = entityList->begin(); it != entityList->end(); it++)
	{
		(*generators)[type]->Register((*it).first, (*entityList)[(*it).first]);
	}
}

void SimulationManager::RegisterSpring(int idSource, int idOther)
{
	if (entityList->find(idSource) != entityList->end() && entityList->find(idOther) != entityList->end() )
	{
		((ForceGeneratorSpring*)(*generators)[SPRING])->Register(idSource, (*entityList)[idSource], (*entityList)[idOther]);
	}
}

void SimulationManager::RemoveEntity(int id)
{
	map<int, Entity*>::iterator it = entityList->find(id);
	if (it != entityList->end())
	{
		entityList->erase(it);
	}
}

void SimulationManager::Update(float time)
{
	vector<int> dropList;

	map<int, Entity*>::iterator cur = entityList->begin();
	map<int, Entity*>::iterator end = entityList->end();
	map<int, Entity*>::iterator swap;

	int id;
	Entity* entity;
	//Process for all entities
	while(cur != end)
	{
		id = cur->first;
		entity = cur->second;
		//Check constraint bounding volume
		if ((*constraints)[GEOMETRIC])
		{
			if(entity->Position()->y < boundVolMin->y)
			{
				entity->Position()->y = boundVolMin->y;
				entity->Invert(Y);
				//entity->Velocity()->y = 0;
				entity->ReduceForce();
			}

			if(entity->Position()->x < boundVolMin->x)
			{
				entity->Position()->x = boundVolMin->x;
				entity->Invert(X);
				entity->ReduceForce();

			}



			if(entity->Position()->y > boundVolMax->y)
			{
				entity->Position()->y = boundVolMax->y;
				entity->Invert(Y);
				entity->ReduceForce();

			}

			if(entity->Position()->x > boundVolMax->x)
			{
				entity->Position()->x = boundVolMax->x;
				entity->Invert(X);
				entity->ReduceForce();

			}
		}

		//Set entity to inital force
		entity->Init();
		
		//Process force generators

		(*generators)[GRAVITY]->Update(id, time);
		//(*generators)[DRAG]->Update(id, time);

		//Update position and velocity
		entity->Update(time);
		//Reset force
		entity->AccumForce()->Zero();
		
		//Check constraint time
		if ((*constraints)[TIME])
		{
			if (entity->LifeTime() > lifeTimeLimit)
			{
				dropList.push_back(id);
			}
		}
		cur++;
	}

	//Remove all entities over life-time
	for (unsigned int i=0; i<dropList.size(); i++)
	{
		RemoveEntity(dropList[i]);
	}
}